// Magnetic Sensor Data Processing
// WQBB 2021-1-27 ~ 2021-2-19

#include <math.h>
#include "msdp.h"

#define MSDP_LOG(format, ...) __OSAL_LOG("[msdp.c] " C_YELLOW format C_NONE, ##__VA_ARGS__)
#define MSDP_LOG1(format, ...) __OSAL_LOG(C_BLUE format C_NONE, ##__VA_ARGS__)

#define MAX(a, b) (((a) > (b)) ? (a) : (b)) /// 两者取大值
#define MIN(a, b) (((a) < (b)) ? (a) : (b)) /// 两者取小值
#define CASE_RETURN_STR(enum_val) \
    case enum_val:                \
        return #enum_val;

#define MSDP_INIT_FLAG 0XAA         /// 门磁标志位
#define MSDP_BASE_DATA_MIN_QTY 60   /// 最少采样60 个基础数据
#define MSDP_BASE_DATA_MAX_QTY 300  /// 最多采样300个基础数据
#define MSDP_BASE_DATA_MIN_DIFF 100 /// 采样数据三个轴的差异之和的最小值

#define MSDP_HYSTERESIS 2           /// 迟滞系数 hysteresis
#define MSDP_AUTO_CALI_SAMPLE_QTY 3 /// 自动校准时的采样点数

/* 门磁当前状态枚举 */
typedef enum
{
    MSDP_STA_DISABLE,         // 禁用
    MSDP_STA_SAMPLE_PARAM,    // 采样状态（计算RAW_PARAM）
    MSDP_STA_SAMPLE_BASE,     // 采样状态（采样BASE-DATA）
    MSDP_STA_WAIT_HALF_CLOSE, // 等待状态（等待用户操作虚掩）
    MSDP_STA_WAIT_ENABLE,     // 等待状态（等待使能门磁）
    MSDP_STA_NORMAL,          // 已使能（正常状态）
    MSDP_STA_AUTO_CALI,       // 自校准状态
} state_t;

/* 门磁基数数据结构体 */
static struct
{
    uint8_t init_flag;

    state_t status;                               // 当前msdp状态
    int16_t offset[3];                            // sensor三个轴的偏移量

    float raw_param;                              // 原始数据的比例参数
    int16_t pos_half_close;                       // 虚掩位置
    int16_t base_len;                             // 基础波形数据长度
    int16_t base_data[MSDP_BASE_DATA_MAX_QTY][3]; // 三轴数据（300个）
} msdp_data = {0}, collect_data = {0};

static int16_t raw_data[3] = {0};   /// 上一个采集点三轴坐标
static uint8_t position_bak = 0xFF; /// 上一个位置
static uint8_t offset_flag = 0;    /// 用于门磁是否添加首次偏移值的标志位
static uint8_t first_offset = 0;   /// 上电磁场首次变化时添加首次偏移值操作的标志位

static int16_t raw_max[3] = {0};    /// 三个轴的原始数据的最大值
static int16_t raw_min[3] = {0};    /// 三个轴的原始数据的最小值
static uint8_t door_state = 0;      /// 门状态：0开门 1关门
static uint8_t last_publish = 0;    /// 门磁异常上报情况--上报过异常不再上报，防止锁睡眠不了
static FunctionalState msensor_func = DISABLE; /// 门磁功能标志位 DISABLE:关 ENABLE:开
static uint8_t iscal_state = 0;     /// 门磁校准状态 0：不在校准中 1：正在门磁校准
static int Msdp_Disable(void);

/**
  * @brief  获取当前门磁状态字符
  * 
  * @note  打印用
  */
static char *Msdp_StatusName(state_t status)
{
    switch (status)
    {
        CASE_RETURN_STR(MSDP_STA_DISABLE);
        CASE_RETURN_STR(MSDP_STA_SAMPLE_PARAM);
        CASE_RETURN_STR(MSDP_STA_SAMPLE_BASE);
        CASE_RETURN_STR(MSDP_STA_WAIT_HALF_CLOSE);
        CASE_RETURN_STR(MSDP_STA_WAIT_ENABLE);
        CASE_RETURN_STR(MSDP_STA_NORMAL);
        CASE_RETURN_STR(MSDP_STA_AUTO_CALI);
    default:
        return NULL;
    }
}

/**
  * @brief  统计三个轴的原始数据的最大值和最小值
  * @note
  *
  * @param  xyz: 原始三轴数据
  */
static void Msdp_RawMaxMin(int16_t x, int16_t y, int16_t z)
{
    int16_t data[3] = {x, y, z};

    for (int axis = 0; axis < 3; axis++)
    {
        if (raw_max[axis] == 30000 || raw_max[axis] < data[axis])
        {
            raw_max[axis] = data[axis];
        }

        if (raw_min[axis] == 30000 || raw_min[axis] > data[axis])
        {
            raw_min[axis] = data[axis];
        }
    }
}

/**
  * @brief  用户正在校准过程中：保存采用数据
  * @note
  *
  * @param  xyz: 压缩后三轴数据
  */
static void Msdp_SaveSample(int16_t x, int16_t y, int16_t z)
{
    if (collect_data.base_len >= MSDP_BASE_DATA_MAX_QTY)
    {
        MSDP_LOG("data too long\t\r\n");
        return;
    }

    for (int i = 0; i < collect_data.base_len; i++)
    {
        // 数据太相似滤掉
        if (abs(collect_data.base_data[i][0] - x) + abs(collect_data.base_data[i][1] - y) + abs(collect_data.base_data[i][2] - z) < 3)
        {
            MSDP_LOG("same sample\t\r\n");
            return;
        }

        // 添加判断，如果三轴偏差突然很大并且偏差后的数值跟之前的数据类似，滤掉,相当于突然往回晃了
        if (abs(collect_data.base_data[collect_data.base_len - 1][0] - x) >= 10 
            || abs(collect_data.base_data[collect_data.base_len - 1][1] - y) >= 10 
            || abs(collect_data.base_data[collect_data.base_len - 1][2] - z) >= 10)
        {
            if (abs(collect_data.base_data[i][0] - x) + abs(collect_data.base_data[i][1] - y) + abs(collect_data.base_data[i][2] - z) <= 5)
            {
                MSDP_LOG("fail data: %d\t%d\t%d\t\r\n", x, y, z);
                return;
            }
        }
    }

    collect_data.base_data[collect_data.base_len][0] = x;
    collect_data.base_data[collect_data.base_len][1] = y;
    collect_data.base_data[collect_data.base_len][2] = z;
    collect_data.base_len++;
    // if (collect_data.base_len % 2 == 0)
    MSDP_LOG("collect: %d\t%d\t%d\t\r\n", x, y, z);
}


/**
  * @brief  基数中的点正常值赋值
  * @note
  *
  * @param  position: 采集波形对应点的位置
  */
static void Msdp_SetBaseData(uint16_t position)
{
    msdp_data.base_data[msdp_data.base_len][0] = collect_data.base_data[position][0];
    msdp_data.base_data[msdp_data.base_len][1] = collect_data.base_data[position][1];
    msdp_data.base_data[msdp_data.base_len][2] = collect_data.base_data[position][2];
    msdp_data.base_len++;
}

/**
  * @brief  基数中的点异常值赋值
  * @note   用上个点的数值
  *
  */
static void Msdp_SetSameBaseData()
{
    msdp_data.base_data[msdp_data.base_len][0] = msdp_data.base_data[msdp_data.base_len - 1][0];
    msdp_data.base_data[msdp_data.base_len][1] = msdp_data.base_data[msdp_data.base_len - 1][1];
    msdp_data.base_data[msdp_data.base_len][2] = msdp_data.base_data[msdp_data.base_len - 1][2];
    msdp_data.base_len++;
}

/**
  * @brief  collect_data清空
  * @note
  *
  * @param
  */
static void Msdp_ClearCollectData()
{
    memset(collect_data.base_data, 0, collect_data.base_len * 3 * sizeof(int16_t));
    collect_data.base_len = 0;
}

/**
  * @brief  对采集的数据进行过滤
  * @note   collect的数据滤成完整的基数msdp_data
  *
  * @param
  */
static void Msdp_ScreenSample()
{
    int16_t collect_max[3], collect_min[3];    // 最大最小值
    int16_t highest[3] = {0}, bottom[3] = {0}; // 最大最小值的位置
    uint8_t big_axis = 0;                      // 最大变化的轴
    uint8_t false_count = 0;

    // 寻找数组最大值和最小值
    for (uint16_t i = 0; i < collect_data.base_len; i++)
    {
        if (i == 0)
        {
            for (uint8_t count = 0; count < 3; count++)
            {
                collect_max[count] = collect_data.base_data[0][count];
                collect_min[count] = collect_data.base_data[0][count];
            }
        }
        else
        {
            for (uint8_t count = 0; count < 3; count++)
            {
                // 获取最大的值的位置
                if (collect_max[count] < collect_data.base_data[i][count])
                {
                    collect_max[count] = collect_data.base_data[i][count];
                    highest[count] = i;
                }
                // 获取最小的值的位置
                if (collect_min[count] > collect_data.base_data[i][count])
                {
                    collect_min[count] = collect_data.base_data[i][count];
                    bottom[count] = i;
                }
            }
        }
    }

    // 获取最大的轴
    for (uint8_t count = 1; count < 3; count++)
    {
        big_axis = (collect_max[count] - collect_min[count]) > (collect_max[big_axis] - collect_min[big_axis]) ? count : big_axis;
    }

    MSDP_LOG("big_axis: %d, highest: %d, collect_max = %d, bottom: %d, collect_min = %d\r\n",
             big_axis, highest[big_axis], collect_max[big_axis], bottom[big_axis], collect_min[big_axis]);

    Msdp_SetBaseData(0);

    for (uint16_t i = 1; i < collect_data.base_len; i++)
    {
        for (uint8_t cur_axis = 0; cur_axis < 3; cur_axis++)
        {
            if (bottom[cur_axis] < highest[cur_axis]) // 波形为先下后上
            {
                if (i <= bottom[cur_axis]) // 下降趋势那段
                {
                    if (collect_data.base_data[i][cur_axis] - msdp_data.base_data[i - 1][cur_axis] > 0) // up
                    {
                        false_count++;
                    }
                }
                else if (i > bottom[cur_axis] && i <= highest[cur_axis]) // 上升趋势那段
                {
                    if (collect_data.base_data[i][cur_axis] - msdp_data.base_data[i - 1][cur_axis] < 0) // down
                        false_count++;
                }
                else if (i > highest[cur_axis]) // 下降趋势那段
                {
                    if (collect_data.base_data[i][cur_axis] - msdp_data.base_data[i - 1][cur_axis] > 0) // up
                        false_count++;
                }
            }
            else if (bottom[cur_axis] > highest[cur_axis]) // 波形为先上后下
            {
                if (i <= highest[cur_axis]) // 上升趋势那段
                {
                    if (collect_data.base_data[i][cur_axis] - msdp_data.base_data[i - 1][cur_axis] < 0) // down
                        false_count++;
                }
                else if (i > highest[cur_axis] && i <= bottom[cur_axis]) // 下降趋势那段
                {
                    if (collect_data.base_data[i][cur_axis] - msdp_data.base_data[i - 1][cur_axis] > 0) // up
                        false_count++;
                }
                else if (i > bottom[cur_axis]) // 上升趋势那段
                {
                    if (collect_data.base_data[i][cur_axis] - msdp_data.base_data[i - 1][cur_axis] < 0) // down
                        false_count++;
                }
            }
        }

        if (false_count > 1)
        {
            Msdp_SetSameBaseData();
            MSDP_LOG("false_count: %d\t\r\n", i);
        }
        else
            Msdp_SetBaseData(i);
        false_count = 0;
    }
    Msdp_ClearCollectData();
}

/**
  * @brief  判断当前点是否在基数中
  * @note
  *
  * @param xyz: 当前点坐标
  * 
  * @return 匹配到返回当前点在基数中的位置，不匹配返回-1
  */
static int Msdp_MatchBaseData(int16_t x, int16_t y, int16_t z)
{
    uint16_t diff_min = UINT16_MAX;
    int index = -1;

    for (int i = 0; i < msdp_data.base_len; i++)
    {
        uint16_t diff = abs(x - (msdp_data.base_data[i][0] + msdp_data.offset[0])) +
                        abs(y - (msdp_data.base_data[i][1] + msdp_data.offset[1])) +
                        abs(z - (msdp_data.base_data[i][2] + msdp_data.offset[2]));

        if (diff < diff_min)
        {
            diff_min = diff;
            index = i;
        }
    }

    if (msdp_data.status == MSDP_STA_WAIT_ENABLE)
        return index;

    static uint8_t offset_cnt_n[3] = {0}; //负偏差计数 Negative
    static uint8_t offset_cnt_p[3] = {0}; //正偏差计数 Positive
    int16_t data[3] = {x, y, z};
    int different[3], axis;

    for (axis = 0; axis < 3; axis++)
    {
        different[axis] = data[axis] - (msdp_data.base_data[index][axis] + msdp_data.offset[axis]);

        if (abs(different[axis]) >= 10) // 单个轴跟最匹配的点相差10，认为数据异常
        {
            if (first_offset)
            {
                offset_cnt_p[axis] = 0;
                offset_cnt_n[axis] = 0;
                // MSDP_LOG("index: %d, axis : %d, diff : %d\t\r\n", index, axis, abs(different[axis]));
                return -1;
            }
            else
            {
                first_offset = 1;
            }
        }

        if (different[axis] < -2) // 单个轴跟最匹配的点相差2-10，认为数据存在偏差
        {
            offset_cnt_n[axis]++;
            offset_cnt_p[axis] = 0;
        }
        else if (different[axis] > 2)
        {
            offset_cnt_p[axis]++;
            offset_cnt_n[axis] = 0;
        }
        else
        {
            offset_cnt_p[axis] = 0;
            offset_cnt_n[axis] = 0;
        }

        if (offset_cnt_n[axis] > 50) //连续负偏差50+次，则修正偏移值
        {
            offset_cnt_n[axis] = 0;
            msdp_data.offset[axis] -= 2; //修正offset
            OSAL_NvWrite(OSAL_OFFSET(msdp_data, offset[axis]), &msdp_data.offset[axis], sizeof(msdp_data.offset[axis]));
            MSDP_LOG(C_BLUE "Fine-tuning axis_%d offset [-]\r\n" C_NONE, axis);
        }
        else if (offset_cnt_p[axis] > 50) //连续正偏差50+次
        {
            offset_cnt_p[axis] = 0;
            msdp_data.offset[axis] += 2; //修正offset
            OSAL_NvWrite(OSAL_OFFSET(msdp_data, offset[axis]), &msdp_data.offset[axis], sizeof(msdp_data.offset[axis]));
            MSDP_LOG(C_BLUE "Fine-tuning axis_%d offset [+]\r\n" C_NONE, axis);
        }
    }
    return index;
}

/**
  * @brief  查找数据数组中差异最大的轴
  * @note
  *
  * @param data: 数据数组 len: 数据长度
  * 
  * @return 返回差异最大的轴（xyz其中一个）
  */
static int Msdp_FindDiffeMaxAxis(int16_t (*data)[3], int len)
{
    int16_t max[3]; // xyz
    int16_t min[3]; // xyz
    int16_t diff_x, diff_y, diff_z;

    for (int i = 0; i < len; i++)
    {
        for (int axis = 0; axis < 3; axis++)
        {
            if (i == 0)
            {
                max[axis] = data[0][axis];
                min[axis] = data[0][axis];
            }
            else
            {
                max[axis] = MAX(max[axis], data[i][axis]);
                min[axis] = MIN(min[axis], data[i][axis]);
            }
        }
    }

    diff_x = max[0] - min[0];
    diff_y = max[1] - min[1];
    diff_z = max[2] - min[2];

    return (diff_x > diff_y) ? ((diff_x > diff_z) ? 0 : 2) : ((diff_y > diff_z) ? 1 : 2);
}

/**
  * @brief  计算偏差量
  * @note   受磁场干扰后数据整体上移或下移，因此计算偏差值方便重新校准
  *
  * @param sample: 采样计算点
  * 
  * @return 偏差量正确：0  偏差值错误：-1
  */
static int Msdp_CalcBaseDataOffset(int16_t (*sample)[3])
{
    uint16_t match_pos[MSDP_AUTO_CALI_SAMPLE_QTY] = {0};
    uint16_t diff_sum_min = UINT16_MAX;
    int16_t different_data[MSDP_AUTO_CALI_SAMPLE_QTY][MSDP_BASE_DATA_MAX_QTY] = {0};
    int i, j, s, select_axis;

    select_axis = Msdp_FindDiffeMaxAxis(sample, MSDP_AUTO_CALI_SAMPLE_QTY); //数据差异最大的轴 xyz分别为012
    MSDP_LOG("select axis:%d\r\n", select_axis);

    for (i = 0; i < msdp_data.base_len; i++)
    {
        for (s = 0; s < MSDP_AUTO_CALI_SAMPLE_QTY; s++) // 样品中数值差异最大的轴对应的数据跟 数组里对应的轴比较的值
        {
            different_data[s][i] = sample[s][select_axis] - (msdp_data.base_data[i][select_axis] + msdp_data.offset[select_axis]);
        }
    }

    for (i = 0; i < msdp_data.base_len; i++)
    {
        uint16_t match[MSDP_AUTO_CALI_SAMPLE_QTY] = {0};

        match[0] = i;
        for (s = 1; s < MSDP_AUTO_CALI_SAMPLE_QTY; s++) //点数
        {
            for (j = 0; j < msdp_data.base_len; j++)
            {
                if (abs(different_data[0][i] - different_data[s][j]) < 2) //相似度（偏差±1）差距的差
                {
                    match[s] = j; // 要是这个判断一直不成立，就没法给match赋值了
                    break;
                }
            }

            if (j == msdp_data.base_len) // 上面的判断只要有一行数组不成立，就会跳出循环，s不会=3
            {
                break;
            }
        }

        if (s == MSDP_AUTO_CALI_SAMPLE_QTY) // 后面两个数组都有一个跟[0][i]差距不大的数，match[]分别存放这个数的位置
        {
            int match_diff_max[3] = {0}; //所有采样点里面，差异值最大的
            int match_diff_min[3] = {0}; //所有采样点里面，差异值最小的

            for (s = 0; s < MSDP_AUTO_CALI_SAMPLE_QTY; s++)
            {
                for (int axis = 0; axis < 3; axis++) // X Y Z
                {
                    if (s == 0)
                    {
                        match_diff_max[axis] = sample[s][axis] - (msdp_data.base_data[match[s]][axis] + msdp_data.offset[axis]);
                        match_diff_min[axis] = match_diff_max[axis];
                    }
                    else // 找到 三个采样点差异最大的轴里变化差距最小的点跟采样点xyz做差值，找出最大最小差
                    {
                        match_diff_max[axis] = MAX(match_diff_max[axis], sample[s][axis] 
                                                - (msdp_data.base_data[match[s]][axis] + msdp_data.offset[axis]));
                        match_diff_min[axis] = MIN(match_diff_min[axis], sample[s][axis] 
                                                - (msdp_data.base_data[match[s]][axis] + msdp_data.offset[axis]));
                    }
                }
                MSDP_LOG("pos_%d: %-6d\t", s, match[s]);
            }
            MSDP_LOG1("\r\n");

            uint16_t diff_sum = (match_diff_max[0] - match_diff_min[0]) + (match_diff_max[1] - match_diff_min[1]) 
                                + (match_diff_max[2] - match_diff_min[2]);
            if (diff_sum_min > diff_sum)
            {
                diff_sum_min = diff_sum;
                memcpy(match_pos, match, sizeof(match));
            }
        }
    }
    MSDP_LOG("\r\n\r\n");

    if (diff_sum_min <= 6)
    {
        int16_t x_offset = 0, y_offset = 0, z_offset = 0;

        MSDP_LOG("diff_sum_min:%d\r\n", diff_sum_min);
        MSDP_LOG("match_pos[s]: %d, %d, %d\r\n", match_pos[0], match_pos[1], match_pos[2]);

        for (s = 0; s < MSDP_AUTO_CALI_SAMPLE_QTY; s++)
        {
            x_offset += sample[s][0] - (msdp_data.base_data[match_pos[s]][0] + msdp_data.offset[0]);
            y_offset += sample[s][1] - (msdp_data.base_data[match_pos[s]][1] + msdp_data.offset[1]);
            z_offset += sample[s][2] - (msdp_data.base_data[match_pos[s]][2] + msdp_data.offset[2]);
        }
        x_offset /= MSDP_AUTO_CALI_SAMPLE_QTY;
        y_offset /= MSDP_AUTO_CALI_SAMPLE_QTY;
        z_offset /= MSDP_AUTO_CALI_SAMPLE_QTY;
        MSDP_LOG("x_offset:%-6d  y_offset:%-6d  z_offset:%-6d\r\n\r\n", x_offset, y_offset, z_offset);

        msdp_data.offset[0] += x_offset;
        msdp_data.offset[1] += y_offset;
        msdp_data.offset[2] += z_offset;

        // 算出来的偏移值用后面三个采样点做计算，如果三个采样点加了偏移值后跟数组里的匹配，则说明偏移量计算正确
        for (s = MSDP_AUTO_CALI_SAMPLE_QTY; s < MSDP_AUTO_CALI_SAMPLE_QTY * 2; s++)
        {
            if (Msdp_MatchBaseData(sample[s][0], sample[s][1], sample[s][2]) < 0)
            {
                msdp_data.offset[0] -= x_offset;
                msdp_data.offset[1] -= y_offset;
                msdp_data.offset[2] -= z_offset;
                return -1;
            }
        }
        return 0;
    }
    return -1;
}

/**
  * @brief  自动校准传感器漂移
  * @note   采样6个点，前3个点用于计算偏移值，后3个点校验偏移值是否正确（必须是连续变化过程中的点）
  *
  * @param sample: 采样计算点
  * 
  * @return 偏差量正确：0  偏差值错误：-1
  */
static MsdpEvent_enum_t Msdp_AutoCalibration(int16_t x, int16_t y, int16_t z, int diff)
{
    static int16_t sample[MSDP_AUTO_CALI_SAMPLE_QTY * 2][3];
    static int16_t sample_qty = 0;
    static uint32_t timestamp = 0;
    static uint8_t count = 0;
    uint32_t time = OSAL_GetTickCount();

    count = (Msdp_MatchBaseData(x, y, z) >= 0) ? (count + 1) : 0;
    if (count > (msdp_data.base_len / 3)) // 连续 （msdp_data.base_len / 3） 个数在数组范围内，匹配成功，说明当前的数据已经没有漂移了，不需要再做处理
    {
        count = 0;
        sample_qty = 0;
        msdp_data.status = MSDP_STA_NORMAL;
        Msdp_SendStatus(0); // 判断状态是否上报给应用层

        position_bak = 0xFF;
        raw_data[0] = 30000;

        MSDP_LOG("msdp auto calibration ok\r\n");
        offset_flag = 0;
        first_offset = 1;
        last_publish = 0;
        return MSDP_EVT_NULL;
    }

    if (diff > 10 || OSAL_PastTime(time, timestamp) > 500) // 如果时间 > 500，说明采点不连贯，采样数置零；如果偏差和比上一个数值 > 10，置零，重新采样
    {
        sample_qty = 0;
        timestamp = time;
    }
    else
    {
        uint16_t different = UINT16_MAX;

        if (sample_qty > 0)
        {
            different = abs(sample[sample_qty - 1][0] - x) + abs(sample[sample_qty - 1][1] - y) + abs(sample[sample_qty - 1][2] - z);
            // MSDP_LOG("diff: %d, offset_flag = %d, first_offset = %d\r\n", different, offset_flag, first_offset);
        }

        if (different > 30) // 三轴差异和 > 30的才算进 采样点 中
        {
            sample[sample_qty][0] = x;
            sample[sample_qty][1] = y;
            sample[sample_qty][2] = z;
            sample_qty++;
            timestamp = time;
            if (different == UINT16_MAX && sample_qty == 1 && offset_flag == 0)
            {
                MSDP_LOG("first error x: %d y: %d z: %d \r\n", x, y, z);

                int16_t diff[3] = {0};
                uint16_t diff_min = UINT16_MAX;
                int index = -1;

                for (int i = 0; i < msdp_data.base_len; i++)
                {
                    uint16_t diff = abs(x - (msdp_data.base_data[i][0] + msdp_data.offset[0])) +
                                    abs(y - (msdp_data.base_data[i][1] + msdp_data.offset[1])) +
                                    abs(z - (msdp_data.base_data[i][2] + msdp_data.offset[2]));

                    if (diff < diff_min)
                    {
                        diff_min = diff;
                        index = i;
                    }
                }
                diff[0] = x - (msdp_data.base_data[index][0] + msdp_data.offset[0]);
                diff[1] = y - (msdp_data.base_data[index][1] + msdp_data.offset[1]);
                diff[2] = z - (msdp_data.base_data[index][2] + msdp_data.offset[2]);
                MSDP_LOG("diff[0]: %d diff[1]: %d diff[2] : %d first_offset : %d\r\n", diff[0], diff[1], diff[2], first_offset);
                
                if (first_offset == 1)
                {
                    if ((abs(diff[0]) + abs(diff[1]) + abs(diff[2])) > 70)
                    {
                        MSDP_LOG("offset too big, error\r\n");
                        offset_flag = 1;
                        return MSDP_EVT_ERROR;
                    }
                }
                msdp_data.offset[0] += diff[0]; //修正offset
                msdp_data.offset[1] += diff[1]; //修正offset
                msdp_data.offset[2] += diff[2]; //修正offset
                OSAL_NvWrite(OSAL_OFFSET(msdp_data, offset), msdp_data.offset, sizeof(msdp_data.offset));
            }
            MSDP_LOG("sample_qty++\r\n");
        }
        else
        {
            if (offset_flag == 0)
            {
                // MSDP_LOG("diff too small, change offset\r\n");
                Msdp_MatchBaseData(x, y, z);
            }
        }
    }

    if (sample_qty < MSDP_AUTO_CALI_SAMPLE_QTY * 2) // 采样点 <6个 不做处理
    {
        return MSDP_EVT_NULL;
    }
    sample_qty = 0;
    count = 0;

    for (int i = 0; i < MSDP_AUTO_CALI_SAMPLE_QTY * 2; i++)
    {
        MSDP_LOG("sample_%d  %-6d  %-6d  %-6d\r\n", i, sample[i][0], sample[i][1], sample[i][2]);
    }

    if (Msdp_CalcBaseDataOffset(sample) == 0)
    {
        OSAL_NvWrite(OSAL_OFFSET(msdp_data, offset), msdp_data.offset, sizeof(msdp_data.offset));
    }
    return MSDP_EVT_NULL;
}

/**
  * @brief  检测当前传感器位置
  * @note
  *
  * @param xyz: 当前坐标
  * 
  * @return 门磁事件：MSDP_EVT_ERROR异常；MSDP_EVT_NULL无事件
  */
static MsdpEvent_enum_t Msdp_CheckPosition(int16_t x, int16_t y, int16_t z)
{
    static uint8_t error_count = 0;
    int position;
    int index = Msdp_MatchBaseData(x, y, z);

    error_count = (index < 0) ? (error_count + 1) : (0);
    if (error_count > 10) // 连续十个数据异常，状态改为自校准，返回错误事件
    {
        error_count = 0;

        MSDP_LOG("MSDP_EVT_ERROR\r\n");
        msdp_data.status = MSDP_STA_AUTO_CALI;
        Msdp_SendStatus(0); // 判断状态是否上报给应用层
        return MSDP_EVT_ERROR;
    }

    if (index < 0)
    {
        return MSDP_EVT_NULL;
    }

    if (position_bak == 1)
    {
        position = (index < msdp_data.pos_half_close) ? 1 : 0;
    }
    else
    {
        position = (index < (msdp_data.pos_half_close - msdp_data.pos_half_close / MSDP_HYSTERESIS)) ? 1 : 0;
    }

    
    if (position_bak != position)
    {
        position_bak = position;
        if (position == 1)
        {
            door_state = 1;
            return MSDP_EVT_ENTER;
        }
        else
        {
            door_state = 0;
            MSDP_LOG("msensor return 1\r\n");
            return MSDP_EVT_EXIT;
        }
    }
    return MSDP_EVT_NULL;
}

/**
  * @brief  检查优化后的基数波形 base data
  * @note
  * 
  * @return 返回 -1：基数不满足条件 0：基数满足条件
  */
static int Msdp_CheckBaseData(void)
{
    int max[3], min[3];

    if (msdp_data.base_len < MSDP_BASE_DATA_MIN_QTY) // 数据太少异常
    {
        MSDP_LOG("base data error: sample(%d) < %d\r\n", msdp_data.base_len, MSDP_BASE_DATA_MIN_QTY);
        return -1;
    }

    if (msdp_data.base_len == MSDP_BASE_DATA_MAX_QTY) // 数据太多，整个流程可能还没读取完数据，返回异常
    {
        MSDP_LOG("base data error: sample(%d) == %d\r\n", msdp_data.base_len, MSDP_BASE_DATA_MAX_QTY);
        return -1;
    }

    if (msdp_data.raw_param < 2) // 没磁铁
    {
        MSDP_LOG("base data error: Not installed magnet\r\n");
        return -1;
    }

    for (int j = 0; j < 3; j++)
    {
        max[j] = msdp_data.base_data[0][j];
        min[j] = msdp_data.base_data[0][j];
    }

    for (int i = 1; i < msdp_data.base_len; i++) // 获取数组中最大最小的xyz值
    {
        for (int j = 0; j < 3; j++)
        {
            max[j] = MAX(max[j], msdp_data.base_data[i][j]);
            min[j] = MIN(min[j], msdp_data.base_data[i][j]);
        }
    }

    if ((max[0] - min[0]) + (max[1] - min[1]) + (max[2] - min[2]) < MSDP_BASE_DATA_MIN_DIFF) // 三轴最大的变化相加<100 异常，数据变化太平缓
    {
        MSDP_LOG("base data error: max_diff < %d\r\n", MSDP_BASE_DATA_MIN_DIFF);
        return -1;
    }
    return 0;
}

/**
  * @brief  msdp禁能
  *
  * @return
  */
static int Msdp_Disable(void)
{
    msdp_data.init_flag = MSDP_INIT_FLAG;
    msdp_data.status = MSDP_STA_DISABLE;
    Msdp_SendStatus(0); // 判断状态是否上报给应用层
    msdp_data.raw_param = 1;
    msdp_data.base_len = 0;
    msdp_data.pos_half_close = 0;
    memset(msdp_data.offset, 0, sizeof(msdp_data.offset));
    OSAL_NvWrite(0, &msdp_data, sizeof(msdp_data) - sizeof(msdp_data.base_data));

    for (int axis = 0; axis < 3; axis++)
    {
        raw_max[axis] = 30000;
        raw_min[axis] = 30000;
    }
    offset_flag = 0;
    return 0;
}

/**
  * @brief  msdp使能
  *
  * @return
  */
static int Msdp_Enable(void)
{
    if (Msdp_CheckBaseData() != 0) // 采集的数据数量和变化不符合理想
    {
        MSDP_LOG("Msdp_Enable, base data error\r\n");
        Msdp_Disable();
        return -1;
    }

    // 关门到虚掩这段采集的点太少，禁能
    if ((msdp_data.pos_half_close <= 10))
    {
        MSDP_LOG("Msdp_Enable, msdp_data.pos_half_close error, %d\r\n", msdp_data.pos_half_close);
        Msdp_Disable();
        return -1;
    }

    msdp_data.status = MSDP_STA_NORMAL;
    Msdp_SendStatus(0); // 判断状态是否上报给应用层
    OSAL_NvWrite(0, &msdp_data, sizeof(msdp_data));
    offset_flag = 0;
    last_publish = 0;
    MSDP_LOG("Msdp_Enable succeed, msdp_data.base_len:%d,  msdp_data.raw_param:%f\r\n", msdp_data.base_len, msdp_data.raw_param);
    return 0;
}

/**
  * @brief  msdp主逻辑
  *
  * @return 门磁事件
  */
MsdpEvent_enum_t Msdp_Process(int16_t x, int16_t y, int16_t z)
{
    MsdpEvent_enum_t evt = MSDP_EVT_NULL;
    int diff;

    x = (int16_t)((float)x / msdp_data.raw_param + 0.5);
    y = (int16_t)((float)y / msdp_data.raw_param + 0.5);
    z = (int16_t)((float)z / msdp_data.raw_param + 0.5);
    // MSDP_LOG("%d\t%d\t%d\t\r\n", x, y, z);

    if (x >= 30000 || y >= 30000 || z >= 30000)
    {
        // MSDP_LOG("Data invalid (30000)\r\n");
        return evt;
    }

    diff = (abs(raw_data[0] - x) + abs(raw_data[1] - y) + abs(raw_data[2] - z));
    if (diff < 3 && msdp_data.status != MSDP_STA_AUTO_CALI)
    {
        // MSDP_LOG("Data invalid (==)\r\n");
        return evt;
    }
    raw_data[0] = x;
    raw_data[1] = y;
    raw_data[2] = z;

    // MSDP_LOG("%d\t%d\t%d\t%f\r\n", x, y, z, msdp_data.raw_param);
    switch (msdp_data.status)
    {
    case MSDP_STA_NORMAL:
        // MSDP_LOG("MsdpFlag: %d, first_offset: %d\r\n", Get_MsdpFlag(), first_offset);
        if (Get_MsdpFlag() || first_offset == 0)
        {
            evt = Msdp_CheckPosition(x, y, z);
        }
        else if (Get_MsdpFlag() == RESET) // TODO 摘掉磁铁是的变化量要多次实验后确认,关门上锁后变化量过大认为磁铁掉了
        {
            uint16_t index = Msdp_MatchBaseData(x, y, z);
            if (index > msdp_data.base_len / 2 && door_state == 1)
            {
                if (last_publish == 0)
                {
                    evt = MSDP_EVT_ERROR;
                    last_publish = 1;
                }  
                // MSDP_LOG("error index = %d, evt = %d\r\n", index, evt);
            }
        }
        break;

    case MSDP_STA_AUTO_CALI:
        evt = Msdp_AutoCalibration(x, y, z, diff);
        break;

    case MSDP_STA_SAMPLE_BASE:
        Msdp_SaveSample(x, y, z);
        break;

    case MSDP_STA_SAMPLE_PARAM:
        Msdp_RawMaxMin(x, y, z);
        break;

    default:
        break;
    }
    return evt;
}

/**
  * @brief  计算数据压缩的倍数
  *
  * @return 倍数值
  */
static float Msdp_CalcParam(void)
{
    int16_t x_diff, y_diff, z_diff, index;
    float param;

    x_diff = raw_max[0] - raw_min[0];
    y_diff = raw_max[1] - raw_min[1];
    z_diff = raw_max[2] - raw_min[2];

    index = (x_diff > y_diff) ? ((x_diff > z_diff) ? 0 : 2) : ((y_diff > z_diff) ? 1 : 2); // 查找哪个轴的变化最大
    param = (float)(raw_max[index] - raw_min[index]) / (float)300;

    MSDP_LOG("raw_max: %-6d %-6d %-6d\r\n", raw_max[0], raw_max[1], raw_max[2]);
    MSDP_LOG("raw_min: %-6d %-6d %-6d\r\n", raw_min[0], raw_min[1], raw_min[2]);
    MSDP_LOG("index:%d\r\n", index);
    return (param <= 1) ? 1 : param;
}

 /**
  * @brief  用户设置
  *
  * @note
  *         
  * @param  opt：用户操作步骤
  */
int Msdp_Setting(MsdpSetting_enum_t opt)
{
    int ret = 0;

    if (opt == MSDP_SET_DISABLE)
    {
        MSDP_LOG("msdp set disable\r\n");
        ret = Msdp_Disable();
    }
    else if (opt == MSDP_SET_OPEN && msdp_data.status == MSDP_STA_DISABLE) // 禁用下开门：计算数据
    {
        msdp_data.status = MSDP_STA_SAMPLE_PARAM;
        MSDP_LOG("msdp set open\r\n");
    }
    else if (opt == MSDP_SET_CLOSE && msdp_data.status == MSDP_STA_SAMPLE_PARAM) // 计算下关门：采样数据，利用数据最大最小值算出压缩倍数
    {
        msdp_data.status = MSDP_STA_SAMPLE_BASE;
        msdp_data.raw_param = Msdp_CalcParam();
        MSDP_LOG("msdp set close, msdp_data.raw_param:%f\r\n", msdp_data.raw_param);
    }
    else if (opt == MSDP_SET_OPEN && msdp_data.status == MSDP_STA_SAMPLE_BASE) // 采样下开门：等待虚掩状态
    {
        msdp_data.status = MSDP_STA_WAIT_HALF_CLOSE;
        MSDP_LOG("msdp set open\r\n");

        Msdp_ScreenSample();
        for (int i = 0; i < msdp_data.base_len; i++)
        {
            MSDP_LOG("%d\t%d\t%d\t\r\n", msdp_data.base_data[i][0], msdp_data.base_data[i][1], msdp_data.base_data[i][2]);
        }
        MSDP_LOG("msdp_data.base_len:%d\r\n", msdp_data.base_len);
    }
    else if (opt == MSDP_SET_HALF_CLOSE && msdp_data.status == MSDP_STA_WAIT_HALF_CLOSE) // 等待虚掩时虚掩：等待使能门磁，获取虚掩位置
    {
        msdp_data.status = MSDP_STA_WAIT_ENABLE;
        MSDP_LOG("pos_half_close:%d, %d, %d\r\n", raw_data[0], raw_data[1], raw_data[2]);
        msdp_data.pos_half_close = Msdp_MatchBaseData(raw_data[0], raw_data[1], raw_data[2]);
        MSDP_LOG("msdp set half close, msdp_data.pos_half_close:%d\r\n", msdp_data.pos_half_close);
    }
    else if (opt == MSDP_SET_ENABLE && msdp_data.status == MSDP_STA_WAIT_ENABLE) // 等待使能下使能：使能门磁
    {
        MSDP_LOG("msdp set enable\r\n");
        ret = Msdp_Enable();
    }
    else
    {
        ret = -1;
        MSDP_LOG("Operation error:%d\r\n", opt);
    }
    Msdp_SendStatus(0); // 判断状态是否上报给应用层
    MSDP_LOG("msdp_data.status: %s\r\n", Msdp_StatusName(msdp_data.status));
    return ret;
}

/**
 * @brief: 上报门磁状态和门磁校准状态
 * @note 判断 门磁使能/禁能 状态变化 和门磁校准中/未在校准 状态变化
 * @param first_flag 是否首次上电
 */
void Msdp_SendStatus(int first_flag)
{
    static FunctionalState last_func_state = DISABLE;
    static uint8_t last_calstate = 0;
    switch (msdp_data.status)
    {
    case MSDP_STA_SAMPLE_PARAM:
    case MSDP_STA_SAMPLE_BASE:
    case MSDP_STA_WAIT_HALF_CLOSE:
    case MSDP_STA_WAIT_ENABLE:
        iscal_state = 1;
        break;
    default:
        iscal_state = 0;
        break;
    }
    if (msdp_data.status == MSDP_STA_NORMAL || msdp_data.status == MSDP_STA_AUTO_CALI)
    {
        msensor_func = ENABLE;
    }
    else
    {
        msensor_func = DISABLE;
    }

    if (msensor_func != last_func_state || first_flag) // 门磁功能开关发生变化，上报给应用层
    {
        Msensor_SendFuncStatus(msensor_func);
        last_func_state = msensor_func;
    }
    
    if (iscal_state != last_calstate || first_flag) // 门磁校准状态发生变化，上报给应用层
    {
        Msensor_SendCalStatus(iscal_state);
        last_calstate = iscal_state;
    }

    return;
}

/**
  * @brief  msdp初始化
  *
  * @return
  */
void Msdp_Init(void)
{
    if (msdp_data.init_flag == 0)
    {
        OSAL_NvRead(0, &msdp_data, sizeof(msdp_data));
        if (msdp_data.init_flag != MSDP_INIT_FLAG)
        {
            Msdp_Disable();
        }
        MSDP_LOG("msdp_data.status:%d\r\n", msdp_data.status);
        Msdp_SendStatus(1);
    }
}
